/*
 * servo.h
 *
 *  Created on: 2024年3月9日
 *      Author: 陈朝
 */
#ifndef SERVO_H
#define SERVO_H

#include "zf_common_headfile.h"
#include <mutex>
#include <thread>
#include <chrono>

// 舵机参数配置
#define SERVO_PWM_DEVICE   "/dev/zf_device_pwm_servo"
#define SERVO_LEFT_MAX     72.4      // 最大左转角度
#define SERVO_RIGHT_MAX    125.6     // 最大右转角度
#define SERVO_CENTER_ANGLE 99      // 中位角度
//右 81.2
//左106

// 控制参数
// 如果小弯道响应不足，可以增大KP_SMALL或减小threshold
// 如果急弯转不过去，可以增大KP_LARGE或增大threshold
// 如果直道震荡，可以减小KP_SMALL或增大threshold
// 如果急弯过冲，可以减小KP_LARGE或调整微分项
#define ERROR_THRESHOLD    0.3f    // 降低阈值使转向更灵敏
#define KP_SMALL           1.8f    // 减小小偏差比例系数
#define KP_LARGE           2.8f    // 减小大偏差比例系数
#define KP_DIFF            0.3f    // 增加微分系数
#define SMOOTH_FACTOR      0.3f    // 增加平滑系数
// 前置声明结构体
typedef struct pwm_info pwm_info_t;

// 舵机控制结构体
typedef struct {
    pwm_info_t pwm_cfg;
    float current_angle;
    float last_error;
} ServoController;

// 函数声明
void servo_init(ServoController *servo);
void servo_set_angle(ServoController *servo, float angle);
void servo_control_by_error(ServoController *servo, int error, int max_error);
void servo_use();

#endif